/*
 * Copyright (c) 2006-2021, RT-Thread Development Team
 *
 * SPDX-License-Identifier: Apache-2.0
 *
 * Change Logs:
 * Date           Author       Notes
 * 2023-03-14     86188       the first version
 */
#include "head_rtthread.h"
#include "head_user.h"

/*===================================================================*/
    /* external interface */
    PID_HandleTypedef PID_Handle;
/*===================================================================*/

#define run_T   0.01    //unit: s
/*angle error limit (unit：deg)*/
#define Rol_Angle_eLIMIT        200
#define Pit_Angle_eLIMIT        200
#define Yaw_Angle_eLIMIT        200

/*angle pid_output limit (unit：deg)*/
#define  Rol_Angle_oLIMIT       500
#define  Pit_Angle_oLIMIT       500
#define  Yaw_Angle_oLIMIT       500

/*angular velocity error limit (unit：deg/s)*/
#define Rol_Rate_eLIMIT         300
#define Pit_Rate_eLIMIT         300
#define Yaw_Rate_eLIMIT         200

/*angular velocity integration limit (unit：deg/s)*/
#define Rol_Rate_iLIMIT         200
#define Pit_Rate_iLIMIT         200
#define Yaw_Rate_iLIMIT         200

/*angular velocity pid_output limit (unit：deg/s)*/
#define Rol_Rate_oLIMIT         500
#define Pit_Rate_oLIMIT         500
#define Yaw_Rate_oLIMIT         500

static float pid_Update(PID_Struct*pid, float FeedBack)
{
    pid->FeedBack = FeedBack;
    /*-------------------------------------------------------------------*/
    /* error calculate */
    pid->Err = pid->Expect - pid->FeedBack;
    if( pid->eLimit != 0 )
    {
        pid->Err = limit(pid->Err, -pid->eLimit, pid->eLimit);
    }
    /* integral calculate */
    pid->Integrate += pid->dt*pid->Err;
    if( pid->iLimit != 0 )
    {
        pid->Integrate = limit(pid->Integrate, -pid->iLimit, pid->iLimit);
    }
    /* differential calculation */
    pid->Dis_Err = (pid->Err - pid->Prev_Err)/pid->dt;
    /*-------------------------------------------------------------------*/
    /*output calculation*/
    pid->OutPut = pid->Kp*(pid->Err) + pid->Ki*(pid->Integrate) + pid->Kd*(pid->Dis_Err);
    pid->OutPut = limit(pid->OutPut, -pid->OutPut_Limit, pid->OutPut_Limit);

    pid->Prev_FeedBack = pid->FeedBack;
    pid->Prev_Err = pid->Err;
    return (pid->OutPut);
}
static float pid_angle_Update(PID_Struct*pid,float FeedBack,float Dis_FeedBack)
{
    pid->FeedBack = FeedBack;
    /*-------------------------------------------------------------------*/
    /* error calculate */
    pid->Err = pid->Expect - pid->FeedBack;
    if( pid->eLimit != 0 )
    {
        pid->Err = limit(pid->Err, -pid->eLimit, pid->eLimit);
    }
    /* integral calculate */
    pid->Integrate += pid->dt*pid->Err;
    if( pid->iLimit != 0 )
    {
        pid->Integrate = limit(pid->Integrate, -pid->iLimit, pid->iLimit);
    }
    /* differential calculation */
    pid->Dis_Err = -Dis_FeedBack;
    /*-------------------------------------------------------------------*/
    /*output calculation*/
    pid->OutPut = pid->Kp*(pid->Err) + pid->Ki*(pid->Integrate) + pid->Kd*(pid->Dis_Err);
    pid->OutPut = limit(pid->OutPut, -pid->OutPut_Limit, pid->OutPut_Limit);

    pid->Prev_FeedBack = pid->FeedBack;
    pid->Prev_Err = pid->Err;
    return (pid->OutPut);
}
static float Yaw_angle_Update(PID_Struct *pid, float FeedBack)
{
    pid->FeedBack = FeedBack;
    /*-------------------------------------------------------------------*/
    /* error calculate */
    pid->Err = pid->Expect - pid->FeedBack;
    /* correct yaw error */
    if(pid->Err > +180)  pid->Err -= 360;
    if(pid->Err < -180)  pid->Err += 360;

    /* integral calculate */
    pid->Integrate += pid->dt*pid->Err;
    if(pid->iLimit != 0)
    {
        pid->Integrate=constrainf(pid->Integrate,-pid->iLimit,pid->iLimit);
    }
    pid->Dis_Err = (pid->Err - pid->Prev_Err)/pid->dt;
    /*-------------------------------------------------------------------*/
    /*output calculation*/
    pid->OutPut = pid->Kp*(pid->Err) + pid->Ki*(pid->Integrate) + pid->Kd*(pid->Dis_Err);
    pid->OutPut = limit(pid->OutPut, -pid->OutPut_Limit, pid->OutPut_Limit);

    pid->Prev_FeedBack = pid->FeedBack;
    pid->Prev_Err = pid->Err;
    return (pid->OutPut);
}
static float error_Update(PID_Struct *pid, float error)
{
    /*-------------------------------------------------------------------*/
    /* error calculate */
    pid->Err = error;
    if( pid->eLimit != 0 )
    {
        pid->Err = limit(pid->Err, -pid->eLimit, pid->eLimit);
    }
    /* integral calculate */
    pid->Integrate += pid->dt*pid->Err;
    if(pid->iLimit != 0)
    {
        pid->Integrate=constrainf(pid->Integrate,-pid->iLimit,pid->iLimit);
    }
    pid->Dis_Err = (pid->Err - pid->Prev_Err)/pid->dt;
    /*-------------------------------------------------------------------*/
    /*output calculation*/
    pid->OutPut = pid->Kp*(pid->Err) + pid->Ki*(pid->Integrate) + pid->Kd*(pid->Dis_Err);
    pid->OutPut = limit(pid->OutPut, -pid->OutPut_Limit, pid->OutPut_Limit);

    pid->Prev_FeedBack = pid->FeedBack;
    pid->Prev_Err = pid->Err;
    return (pid->OutPut);
}

static void pidInit(PID_Struct* pid, float kp, float ki, float kd, \
                    int16_t eLimit, int16_t iLimit, int16_t outputLimit, float dt)
{
    pid->Expect     = 0;
    pid->FeedBack   = 0;
    pid->Err        = 0;
    pid->Prev_Err   = 0;
    pid->Dis_Err    = 0;
    pid->Integrate  = 0;
    pid->OutPut     = 0;
    pid->eLimit     = eLimit;
    pid->iLimit     = iLimit;
    pid->OutPut_Limit = outputLimit;
    pid->Kp           = kp;
    pid->Ki           = ki;
    pid->Kd           = kd;
    pid->dt           = dt;
    pid->enableDFilter= 0;
}
static void pidReset(PID_NUM num)
{
    PID_Handle.pid[num].Expect   = 0;
    PID_Handle.pid[num].Err      = 0;
    PID_Handle.pid[num].Prev_Err = 0;
    PID_Handle.pid[num].Integrate= 0;
    PID_Handle.pid[num].Dis_Err  = 0;
    PID_Handle.pid[num].OutPut   = 0;
}

/* reset contral data */
static void pidReset2(void)
{
    pidReset(Rol_Angle);
    pidReset(Pit_Angle);
    pidReset(Yaw_Angle);

    pidReset(Rol_Rate);
    pidReset(Pit_Rate);
    pidReset(Yaw_Rate);

    pidReset(X_Position);
    pidReset(Y_Position);
    pidReset(Z_Position);
    pidReset(X_Speed);
    pidReset(Y_Speed);
    pidReset(Z_Speed);

}
static void ALL_PidInit(void)
{
    PID_Struct *pid = PID_Handle.pid;

    pidInit(&pid[Rol_Angle], Parame_PID.kp_[Rol_Angle], Parame_PID.ki_[Rol_Angle], Parame_PID.kd_[Rol_Angle], Rol_Angle_eLIMIT, 0, Rol_Angle_oLIMIT , run_T);
    pidInit(&pid[Pit_Angle], Parame_PID.kp_[Pit_Angle], Parame_PID.ki_[Pit_Angle], Parame_PID.kd_[Pit_Angle], Pit_Angle_eLIMIT, 0, Pit_Angle_oLIMIT , run_T);
    pidInit(&pid[Yaw_Angle], Parame_PID.kp_[Yaw_Angle], Parame_PID.ki_[Yaw_Angle], Parame_PID.kd_[Yaw_Angle], Yaw_Angle_eLIMIT, 0, Yaw_Angle_oLIMIT , run_T);

    pidInit(&pid[Rol_Rate] , Parame_PID.kp_[Rol_Rate], Parame_PID.ki_[Rol_Rate], Parame_PID.kd_[Rol_Rate], Rol_Rate_eLIMIT, Rol_Rate_iLIMIT, Rol_Rate_oLIMIT, run_T);
    pidInit(&pid[Pit_Rate] , Parame_PID.kp_[Pit_Rate], Parame_PID.ki_[Pit_Rate], Parame_PID.kd_[Pit_Rate], Pit_Rate_eLIMIT, Pit_Rate_iLIMIT, Pit_Rate_oLIMIT, run_T);
    pidInit(&pid[Yaw_Rate] , Parame_PID.kp_[Yaw_Rate], Parame_PID.ki_[Yaw_Rate], Parame_PID.kd_[Yaw_Rate], Yaw_Rate_eLIMIT, Yaw_Rate_iLIMIT, Yaw_Rate_oLIMIT, run_T);

    pidInit(&pid[X_Position] , Parame_PID.kp_[X_Position], Parame_PID.ki_[X_Position], Parame_PID.kd_[X_Position], Rol_Rate_eLIMIT, Rol_Rate_iLIMIT, Rol_Rate_oLIMIT, run_T);
    pidInit(&pid[X_Speed] , Parame_PID.kp_[X_Speed], Parame_PID.ki_[X_Speed], Parame_PID.kd_[X_Speed], Rol_Rate_eLIMIT, Rol_Rate_iLIMIT, Rol_Rate_oLIMIT, run_T);
    pidInit(&pid[Y_Position] , Parame_PID.kp_[Y_Position], Parame_PID.ki_[Y_Position], Parame_PID.kd_[Y_Position], Rol_Rate_eLIMIT, Rol_Rate_iLIMIT, Rol_Rate_oLIMIT, run_T);
    pidInit(&pid[Y_Speed] , Parame_PID.kp_[Y_Speed], Parame_PID.ki_[Y_Speed], Parame_PID.kd_[Y_Speed], Rol_Rate_eLIMIT, Rol_Rate_iLIMIT, Rol_Rate_oLIMIT, run_T);
    pidInit(&pid[Z_Position] , Parame_PID.kp_[Z_Position], Parame_PID.ki_[Z_Position], Parame_PID.kd_[Z_Position], Rol_Rate_eLIMIT, Rol_Rate_iLIMIT, Rol_Rate_oLIMIT, run_T);
    pidInit(&pid[Z_Speed] , Parame_PID.kp_[Z_Speed], Parame_PID.ki_[Z_Speed], Parame_PID.kd_[Z_Speed], Rol_Rate_eLIMIT, Rol_Rate_iLIMIT, 350, run_T);

    pidInit(&pid[stableRol] , Parame_PID.kp_[stableRol], Parame_PID.ki_[stableRol], Parame_PID.kd_[stableRol], Rol_Rate_eLIMIT, Rol_Rate_iLIMIT, Rol_Rate_oLIMIT, run_T);
    pidInit(&pid[stablePit] , Parame_PID.kp_[stablePit], Parame_PID.ki_[stablePit], Parame_PID.kd_[stablePit], Rol_Rate_eLIMIT, Rol_Rate_iLIMIT, Rol_Rate_oLIMIT, run_T);
}

static int pidHandle_init(void)
{
    PID_Handle.Update       = pid_Update;
    PID_Handle.angle_Update = pid_angle_Update;
    PID_Handle.Yaw_angle_Updata = Yaw_angle_Update;
    PID_Handle.pidReset     = pidReset;
    PID_Handle.pidReset2    = pidReset2;
    PID_Handle.All_PidInit  = ALL_PidInit;
    PID_Handle.error_Updata = error_Update;

    return 0;
}
INIT_APP_EXPORT(pidHandle_init);
